/*
 * imu.c
 *
 *  Created on: Apr 7, 2024
 *      Author: boen
 */

#include <imu.h>
static float imu_yaw = 0;
static float imu_delta = 0;
static uint8_t imu_run = 0;
static uint16_t imu_tick = 0;


/**
* @brief
* @param
* @retval
*/
static float convert_to_pi(float angle) {
//    if (angle > M_PI) {
//        return angle - 2 * M_PI;
//    } else {
//        return angle;
//    }
	float yaw;
	yaw = angle - imu_delta;
	if(yaw>M_PI)
	{
		yaw -= 2 * M_PI;
	}
	if(yaw<-M_PI)
	{
		yaw += 2 * M_PI;
	}
	return yaw;
}


/**
* @brief
* @param
* @retval
*/
void imu_timer(void)
{
	if(imu_tick)
	{
		imu_tick--;
	}
}


/**
* @brief
* @param
* @retval
*/
int imu_get_reply(float yaw)
{
	imu_yaw = yaw;
	imu_run = 1;
	return 0;
}


/**
* @brief
* @param
* @retval
*/
int imu_is_ready(void)
{
	return xbot.imu.state;
}


/**
* @brief
* @param
* @retval
*/
int imu_init(void)
{
	xbot.imu.state = SYS_FLAG_STARTUP;
	imu_tick = 5000;
	usart4_init();
#if DETA10
	static uint16_t imu_num = 0;
	float tmp;
	while(1)
	{
		if(imu_run)
		{
			tmp = imu_yaw;
			if (tmp<(imu_delta+0.003f)&&(tmp>(imu_delta-0.003f)))
			{
				imu_num++;
			}
			else
			{
				imu_delta = tmp;
				imu_num = 0;
			}
			if(imu_num>20)
			{
				//xbot.imu.state = SYS_FLAG_PLAY;
				break;
			}
		}
		osDelay(100u);
	}
#endif
#if JY901S
	while(1)
	{
		if(imu_run)
		{
			xbot.imu.state = SYS_FLAG_PLAY;
			imu_delta = imu_yaw;
			printf("[imu]: imu_is_ready. delta->(%f)\r\n", imu_delta);
			break;
		}
		if(imu_tick == 0)
		{
			printf("[imu]: imu_is_faild. \r\n");
			break;
		}
	}
#endif
	return 0;
}


/**
* @brief
* @param
* @retval
*/
int imu_deinit(void)
{
	xbot.imu.state = SYS_FLAG_SHUTDOWN;
	return 0;
}


/**
* @brief
* @param
* @retval
*/
int imu_polling(void)
{
	if(xbot.imu.state == SYS_FLAG_PLAY)
	{
#if DETA10
		xbot.mobile.pose.angular.Z = (convert_to_pi(imu_yaw))*(-1); // IMU的坐标系为NED，所以需要反转Y
#endif
#if JY901S
		xbot.mobile.pose.angular.Z = (convert_to_pi(imu_yaw)); // 右手坐标系
#endif
		//printf("Z:%f yaw:%f err:%f\r\n", xbot.mobile.pose.angular.Z, imu_yaw, imu_delta);
	}
	return 0;
}

